#include "Math/Vector3.h"

#include "Math/Quaternion.h"

namespace tyro
{
	namespace Math
	{
		template < class T >
		const Vector3<T> Vector3<T>::ZERO
		(
			0.0f, 0.0f, 0.0f
		); 

		template < class T >
		const Vector3<T> Vector3<T>::UNIT_SCALE
		(
			1.0f, 1.0f, 1.0f
		);

		template < class T >
		const Vector3<T> Vector3<T>::UNIT_X
		(
			1.0f, 0.0f, 0.0f
		);

		template < class T >
		const Vector3<T> Vector3<T>::UNIT_Y
		(
			0.0f, 1.0f, 0.0f
		);

		template < class T >
		const Vector3<T> Vector3<T>::UNIT_Z
		(
			0.0f, 0.0f, 1.0f
		);

		template < class T >
		const Vector3<T> Vector3<T>::NEG_UNIT_X
		(
			-1.0f, 0.0f, 0.0f
		);

		template < class T >
		const Vector3<T> Vector3<T>::NEG_UNIT_Y
		(
			0.0f, -1.0f, 0.0f
		);

		template < class T >
		const Vector3<T> Vector3<T>::NEG_UNIT_Z
		(
			0.0f, 0.0f, -1.0f
		);


		template <class T>
		Quaternion<T> tyro::Math::Vector3<T>::GetRotationArc( const Vector3<T>& dest, const Vector3<T>& axis /*= Vector3<T>::ZERO*/ ) const
		{
			Quaternion<T> q;

			Vector3<T> v0 = *this;
			Vector3<T> v1 = dest;
			v0.Normalise();
			v1.Normalise();


			T d = v0.Dot(v1);

			// Return if the same
			if (d >= 1.0f)
			{
				return Quaternion<T>::IDENTITY;
			}

			if (d < (1e-6f - 1.0f))
			{
				if (axis != Vector3<T>::ZERO)
				{
					q.FromAngleAxis(Radian<T>(Math::PI), axis);
				}
				else
				{
					Vector3<T> genAxis = Vector3<T>::UNIT_X.CrossProduct(*this);
					if (genAxis.IsZeroLength())
					{
						genAxis = Vector3<T>::UNIT_Y.CrossProduct(*this);
						
					}
					genAxis.Normalise();

					q.FromAngleAxis(Radian<T>(Math::PI), genAxis);

				}

			}
			else
			{
				T s = Math::Sqrt( (1+d)*2 );
				T invs = 1 / s;

				Vector3<T> c = v0.CrossProduct(v1);

				q.x = c.x * invs;
				q.y = c.y * invs;
				q.z = c.z * invs;
				q.w = s * 0.5f;
				q.Normalise();
			}


			return q;

		}

		template class Vector3< tyro::FLOAT_32 >;
		template class Vector3< tyro::FLOAT_64 >;

		


		


		

	}
}